package jp.seraph.robocup.soccer.sample.agent;

import jp.seraph.jsade.core.Player;
import jp.seraph.jsade.math.Angle;
import jp.seraph.jsade.math.AngleVelocity;
import jp.seraph.jsade.model.AngleVelocityCalculator;
import jp.seraph.jsade.model.JointIdentifier;

public class SampleCalculator implements AngleVelocityCalculator {
    public void setGain(double aGain){
        mGain = aGain;
    }

    private double mGain = 0.05;
    private static final double EPS = 0.2;

    
    public AngleVelocity calc(Player aPlayer, JointIdentifier aTargetJoint, Angle aTargetAngle) {
        Angle tCurrentAngle =aPlayer.getJointAngle(aTargetJoint);
        double tDiff = tCurrentAngle.subtract(aTargetAngle).abs().getDegree();
        if(tDiff <= EPS)
            return AngleVelocity.ZERO;

        double tResult = mGain * (aTargetAngle.getDegree() - tCurrentAngle.getDegree());

        // 元の式はDegreeで計算してるので、無理矢理合わせる
        return AngleVelocity.createAsRad(tResult);
    }
}
